<!DOCTYPE html>
<html lang="zh">
  <head>
    <meta charset="utf-8">
    <meta name="viewport" content="width=device-width, initial-scale=1">
    <link rel="apple-touch-icon" href="apple-touch-icon.png">
    <link rel="shortcut icon" href="favicon.ico" />
    <link rel="manifest" href="a3.webmanifest">
    <title>A3Class UR5</title>
  </head>
  <body>
    <link rel="stylesheet" href="css/main.css"/>
    <script src="js/Theme.js"></script>
    <script src="js/ammo.js/build/ammo.wasm.js"></script>
    <div class="div-fullscreen"><div id="container"></div></div>
    <script type="module">
      import * as THREE from "./js/three.js/build/three.module.js";
      import URDFLoader from './jsm/URDFLoader/URDFLoader.js';

      import A3Class from './js/A3Class.js';
      import A3Actor from './js/A3Actor.js';
      import A3BasicShape from './js/A3BasicShape.js';

      const container = document.getElementById( 'container' );

      const a3 = new A3Class( container );

      await a3.initAmmo();
      a3.init();

      const pos = new THREE.Vector3();
      const quat = new THREE.Quaternion( 0, 0, 0, 1 );

      const box = new A3BasicShape("Box");
      a3.addActor(box);
      pos.set( 2, 0.5, 0 );
      box.setPosition( pos );

      const cylinder = new A3BasicShape("Cylinder");
      a3.addActor(cylinder);
      pos.x += 1;
      cylinder.setPosition( pos );

      const sphere = new A3BasicShape("Sphere");
      a3.addActor(sphere);
      pos.x += 1;
      sphere.setPosition( pos );

      const cone = new A3BasicShape("Cone");
      a3.addActor(cone);
      pos.x += 1;
      cone.setPosition( pos );

      const capsule = new A3BasicShape("Capsule");
      pos.x += 1;
      pos.y = 1;
      capsule.setPosition( pos );      
      a3.addActor(capsule);
      // capsule.threeObject.material.wireframe = true;

      // 游泳圈
      // const torusGeometry = new THREE.TorusGeometry(2, .2, 24, 24);
      // const torusMesh = new THREE.Mesh( torusGeometry );
      // const torus = new A3Actor();
      // quat.setFromAxisAngle(a3.AxisX, Math.PI / 2);
      // torus.setFromMesh( torusMesh ,1, pos, quat );
      // torus.setMaterial();
      const torus = new A3BasicShape("Torus", 1, .49);
      // pos.x += 2;
      // pos.y += 1;
      // // quat.setFromAxisAngle(a3.AxisX, Math.PI / 2);
      // pos.set( 0, 0, )
      // torus.setPosition( pos );
      // torus.setQuaternion( quat );
      a3.addActor(torus)

      a3.addGround(40, 0.1, 40, 'textures/grid.png');

      a3.removeActor( cone );

      const boxSize = 1;

      for(var x = 0; x < 5; x ++){
        for(var y = 0; y < 10; y++){
          const box = new a3.A3BasicShape( "Box", boxSize * 2, boxSize, boxSize, 1 );
          pos.set( x * boxSize* 2 + boxSize - 5, y * boxSize + boxSize / 2, -3);
          box.setPosition(pos);
          a3.addActor(box);
        }
      }

      // const box = new a3.A3BasicShape( "Box", 1, 1, 1, 1 );
      // pos.y = 20;
      // box.setPosition(pos);
      // a3.addActor(box)

      // 加载URDF
      let ur5;
      const manager = new THREE.LoadingManager();

      manager.onLoad = function ( ) { 
        console.log( 'Loading complete!');

        a3.disableMeshLight( ur5 );

        ur5.quaternion.setFromAxisAngle( a3.AxisX, -Math.PI / 2 );
        ur5.setJointValue('shoulder_lift_joint', -Math.PI / 2);
        ur5.setJointValue('wrist_1_joint', -Math.PI / 2)

        const ur5Actor = new a3.URDFHelper( ur5, 10 );
        console.log( ur5Actor );
        ur5Actor.setPosition ( 0, 1, 0 );
        a3.addActor( ur5Actor );
        ur5Actor.threeObject.position.x -= 5;
        // a3.addMesh( ur5 );
      }

      const loader = new URDFLoader( manager );
      loader.parseCollision = true;
      loader.parseInertial = true;
      const ur5UrdfFile = 'files/ur_description/urdf/ur5_robot.urdf';

      loader.packages = {
        ur_description:'files/ur_description',
      };

      loader.load(ur5UrdfFile, robot => {
        ur5 = robot;
      });

      a3.start();
      animate();

      var jointValue = 0;
      var dir = 1;

      function animate() {

        requestAnimationFrame( animate );

        if( ur5 ) {

          ur5.setJointValue( 'shoulder_pan_joint', jointValue );
          ur5.setJointValue( 'elbow_joint', jointValue);
          
          if( jointValue > Math.PI / 2 ) {
            dir = -1;
          }

          if( jointValue < -Math.PI / 2 ) {
            dir = 1;
          }

          jointValue += dir * 0.02;

          // capsule.enableSimulation( false );
        }

      }

    </script>
  </body>
</html>